#include "positionloop.hpp"
#include <iostream>

int main()
{
    double ts = 0.001;
    PositionLoop pl(ts);
    int refer=0;
    int current = 0;
    pl.initPidController_parallel(current, 5.0e-3);
    pl.initRcFilter(2.0, current);
    pl.initVelEst(5.0e3);
    std::cout <<"up:" <<std::endl;
    for(int i=0; i<100; i++)
    {
        refer += 100;
        current += pl.loopOut(refer, current) * ts;
        std::cout << "=" << i<<"=loopOut:"<< refer<< " " << current <<std::endl;
    }
    std::cout <<"hold:" <<std::endl;
    for(int i=100; i<200; i++)
    {
        current += pl.loopOut(refer, current) * ts;
        std::cout << "=" << i<<"=loopOut:"<< refer<< " " << current <<std::endl;
    }
}
